/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.archwood.frc2607.logomotion;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Timer;

/**
 *
 * @author Ron
 */
public class LogomotionArm implements LogomotionConstants {

    private AnalogChannel elbowPot = null, shoulderPot = null;
    private CANJaguar elbowLeftMotor = null, elbowRightMotor = null,
                     shoulderLeftMotor = null, shoulderRightMotor = null;
    private Solenoid theClaw = null;            // off means closed, on means opens
    private short elbowLoopCount = 0, shoulderLoopCount = 0;
    private double elbowSpeed, shoulderSpeed;
    private int shoulderPos;
    public LogomotionArm() {

        initMotors();
        elbowLoopCount = 0;
        shoulderLoopCount = 0;
        shoulderPos = 0;
        elbowSpeed = 0;
        shoulderSpeed = 0;
        elbowPot = new AnalogChannel(elbowPotAI);
        shoulderPot = new AnalogChannel(shoulderPotAI);
        theClaw = new Solenoid(clawSolenoid);
        theClaw.set(false);
    }

    public boolean gotoPosition(double shoulder, double elbow) {
        // return true when at position, false when moving to position
        double elbowPos = elbowPot.getAverageValue(),
               shoulderPos = shoulderPot.getAverageValue();

        // elbowPos increases as elbow rotates outwards (speed > 0)
        // elbowPos decreases as elbow rotates inwards (speed < 0)
        // if desired elbow position < current position, rotate inwards
        // if desired elbow position > current position, rotate outwards
        // err = elbow - elbowPos, sign matches speed
        // if abs(err) is within 10, stop
        double elbowErr = elbow - elbowPos;
        double elbowSpeed = 0.0;
        boolean done = true;
        if (Math.abs(elbowErr) > 10) {
            if (elbowErr < 0) elbowSpeed = -.3;
            else elbowSpeed = .3;
            done = false;
        }

        // shoulderPos increases as shoulder raises (speed < 0)
        // shoulderPos decreases as shoulder lowers (speed > 0)
        // if desired shoulder position < current position, lower shoulder
        // if desired shoulder position > current position, raise shoulder
        // err = shoulderPos - shoulder, sign matches speed
        double shoulderErr = shoulderPos - shoulder;
        double shoulderSpeed = 0.0;
        if (Math.abs(shoulderErr) > 10) {
            if (shoulderErr < 0) shoulderSpeed = -.5;
            else shoulderSpeed = .5;
            done = false;
        }
        setElbowSpeed(elbowSpeed);
        setShoulderSpeed(shoulderSpeed);
        System.out.println("elbowSpeed = " + elbowSpeed + "shoulderSpeed = " +
                           shoulderSpeed + (done ? "true" : "false"));
        return done;
    }


    public void setElbowSpeed(double speed) {

        // speed < 0 rotates elbow inwards (back towards home position)
        // speed > 0 rotates elbow outwards (away from home position)
        
        double elbowPos = elbowPot.getAverageValue();
        elbowSpeed = speed;
        if ((elbowSpeed < 0 && elbowPos <= elbowMinPosition) ||
            (elbowSpeed > 0 && elbowPos >= elbowMaxPosition)) {
            elbowSpeed = 0.0;
            elbowLoopCount = 10;
        }
        if (++elbowLoopCount >= 2) {
            elbowLoopCount = 0;
            try {
                byte syncGroup = 0x05;
                elbowLeftMotor.setX(elbowSpeed, syncGroup);
                elbowRightMotor.setX(elbowSpeed, syncGroup);
                CANJaguar.updateSyncGroup(syncGroup);
            } catch (Exception e) {
                System.out.println("setElbowSpeed() exception...");
            }
        }
    }
    public void openClaw() {
        theClaw.set(true);
    }

    public void closeClaw() {
        theClaw.set(false);
    }

    public void setShoulderSpeed(double speed) {

        // speed < 0 raises shoulder
        // speed > 0 lowers shoulder
/*
        if (speed < 0) ++shoulderPos;
        if (speed > 0) --shoulderPos;
        if (shoulderPos < 0) shoulderPos = 0;
*/
        shoulderSpeed = speed;
        if (++shoulderLoopCount >= 2) {
            shoulderLoopCount = 0;
            try {
                byte syncGroup = 0x06;
                shoulderLeftMotor.setX(shoulderSpeed, syncGroup);
                shoulderRightMotor.setX(shoulderSpeed, syncGroup);
                CANJaguar.updateSyncGroup(syncGroup);
            } catch (Exception e) {
                System.out.println("setShoulderSpeed() exception...");
            }
        }
    }

    public void toggleClaw() {
            theClaw.set(!theClaw.get());  
    }

    public String getPotValues() {
        String e = Integer.toString(elbowPot.getAverageValue());
        String s = Integer.toString(shoulderPot.getAverageValue());
        if (e.length() > 3) e = e.substring(0,3);
        if (s.length() > 3) s = s.substring(0,3);
        return "elb: " +  e + " shld: " + s + "   ";
    }

    private void initMotors() {
        while (elbowLeftMotor == null) {
            try {
                elbowLeftMotor = new CANJaguar(elbowLeftCANID);
                System.out.println("elbowLeft Jag created successfully");
            } catch (Exception e) {
                System.out.println("elbowLeft Jag exception, retrying...");
                Timer.delay(.25);
            }
        }
        while (shoulderLeftMotor == null) {
            try {
                shoulderLeftMotor = new CANJaguar(shoulderLeftCANID);
                System.out.println("shoulderLeft Jag created successfully");
            } catch (Exception e) {
                System.out.println("shoulderLeft Jag exception, retrying...");
                Timer.delay(.25);
            }
        }
        while (elbowRightMotor == null) {
            try {
                elbowRightMotor = new CANJaguar(elbowRightCANID);
                System.out.println("elbowRight Jag created successfully");
            } catch (Exception e) {
                System.out.println("elbowRight Jag exception, retrying...");
                Timer.delay(.25);
            }
        }
        while (shoulderRightMotor == null) {
            try {
                shoulderRightMotor = new CANJaguar(shoulderRightCANID);
                System.out.println("shoulderRight Jag created successfully");
            } catch (Exception e) {
                System.out.println("shoulderRight Jag exception, retrying...");
                Timer.delay(.25);
            }
        }
    }

    public String getTelemetryHeader() {
        return "elbowSpeed,shoulderSpeed,elbowPot,shoulderPot,claw";
    }

    public String getTelemetryData() {
        String val = elbowSpeed + "," + shoulderSpeed + ",";
        val += Integer.toString(elbowPot.getAverageValue()) + ",";
        val += Integer.toString(shoulderPot.getAverageValue()) + ",";
        val += (theClaw.get() ? "open" : "closed");
        return val;        
    }
}
